#include "FOC.h"

int Sensor_DIR = -1; // 传感器方向
int Motor_PP = 7;    // 电机极对数


float receivedFloats[4] = {0.4,0.04,0.000001,100000}; // 用于存储4个浮点数的全局数组
int floatCount = 0;       // 记录已接收的浮点数个数
unsigned long lastReceiveTime = 0; // 用于超时判断
const long TIMEOUT = 50;         // 超时时间(毫秒)
void getFourFloat()
{
  // 当串口有数据可用时[2,4](@ref)
  if (Serial.available() > 0)
  {
    floatCount = 0;             // 重置计数器
    lastReceiveTime = millis(); // 记录开始接收的时间

    // 等待接收4个浮点数或超时
    while (floatCount < 4 && (millis() - lastReceiveTime) < TIMEOUT)
    {
      if (Serial.available() > 0)
      {
        // 使用parseFloat解析浮点数[3,6](@ref)
        receivedFloats[floatCount] = Serial.parseFloat();
        floatCount++;
        lastReceiveTime = millis(); // 每次成功解析都重置超时计时
      }
    }

    // 处理接收到的数据
    if (floatCount == 4)
    {
      Serial.println("成功接收4个浮点数：");
      for (int i = 0; i < 4; i++)
      {
        Serial.print("数据 ");
        Serial.print(i + 1);
        Serial.print(": ");
        Serial.println(receivedFloats[i], 4); // 打印带4位小数的浮点数[5](@ref)
      }
    }
    else
    {
      Serial.println("错误：接收数据不完整或超时。");
    }

    // 清空串口缓冲区[3](@ref)
    while (Serial.available() > 0)
    {
      Serial.read();
    }

    FOC_M0_SET_VEL_PID(receivedFloats[0],receivedFloats[1],receivedFloats[2],receivedFloats[3]);
  }
}

void setup()
{
  Serial.begin(115200);
  pinMode(12, OUTPUT);
  digitalWrite(12, HIGH); // V4电机使能

  FOC_Vbus(12.6); // 设定驱动器供电电压
  FOC_alignSensor(Motor_PP, Sensor_DIR);
  FOC_M0_SET_VEL_PID(receivedFloats[0],receivedFloats[1],receivedFloats[2],receivedFloats[3]);
}

int Times = 0;
// void loop() 
// {
//   FOC_M0_SET_VEL_PID(receivedFloats[0],receivedFloats[1],receivedFloats[2],receivedFloats[3]);

//   Serial.println(FOC_M0_Angle());
//   Serial.printf("%.4f %.6f %.8f %f",receivedFloats[0],receivedFloats[1],receivedFloats[2],receivedFloats[3]);
  

//   FOC_M0_set_Velocity_Angle(1);

//   getFourFloat();
// }

void loop() 
{
  Serial.println(FOC_M0_Angle());

  Times = Times + 1;
  if(Times < 2000) 
  {
    FOC_M0_set_Velocity_Angle(1.0);
  }
  else if (Times < 4000) 
  {
    FOC_M0_set_Velocity_Angle(2);
  }
  else Times = 0;
  
  getFourFloat();
}


// 0.5,0.002,0.001,100000
// 0.4,0.001,0.001,100000
// 0.08,0.003,0.00003,100000 电流环，稳定但是存在稳态误差
// 0.2,0.03,0.00009,100000
// 0.35,0.03,0.000001,100000 电流环，稳定，消除稳态误差
// 0.35,0.03,0.000007,100000
// 0.4,0.03,0.000001,100000

// 0.4,0.04,0.000001,100000 0.4 加入对微分的滤波